#ifndef __HEARTRATE_HPP
#define __HEARTRATE_HPP

// heartrate.hpp
// 包含从一系列颜色数据计算心率的函数

#include <valarray>
#include "misc_processing.hpp"
#include "xiaobo/xiaoboband.h"
#include "xiaobo/xiaoboband_emxAPI.h"

// 根据一段时间的平均脸色计算心率
// 输入应该是指向cv::Vec3x的迭代器
template <typename IT>
double get_heart_rate(IT begin, IT end, double fps);

template <typename IT>
double get_heart_rate(IT begin, IT end, double fps) {
    int n = end - begin;
    int i;
    IT p;

    auto x_s = chrominance(begin, end, 3.0, -2.0, 0.0);
    auto y_s = chrominance(begin, end, 1.5, 1.0, -1.5);

    double alpha = std_dev(std::begin(x_s), std::end(x_s)) / std_dev(std::begin(y_s), std::end(y_s));

    std::valarray<double> chdata = x_s - alpha*y_s;

    chdata = normalized(std::begin(chdata), std::end(chdata), -1.0, 1.0);

    // 小波滤波
    double xiaobodata[128];
    std::valarray<double> chdata_filtered(n);
    emxArray_real_T *y;
    for (int i=0; i<4; ++i) {
        for (int j=0; j<128; ++j) {
            xiaobodata[j] = chdata[128*i+j];
        }
        emxInitArray_real_T(&y, 2);
        xiaoboband(xiaobodata, y);
        auto tempdata = normalized(y->data, y->data+128, -1.0, 1.0);
        chdata_filtered[std::slice(128*i, 128, 1)] = tempdata;
        emxDestroyArray_real_T(y);
    }

    // 对滤波之后的数据进行fft并取绝对值
    auto chdata_fft = opencv_fft1(std::begin(chdata_filtered), std::end(chdata_filtered));
    auto chdata_fft_abs = complex_abs(chdata_fft);

    // 在0.5Hz与4Hz之间寻找峰值
    int lb = int(0.5*n/fps);
    int ub = int(4.0*n/fps);
    int fft_peak = std::max_element(chdata_fft_abs.begin<double>() + lb, chdata_fft_abs.begin<double>() + ub) - chdata_fft_abs.begin<double>();

    return 120 * fft_peak * fps / n;
}

#endif